/*
*	Project: Aethon
*
*	File: Control.cpp
*
*	Author: Jonathan Hersack
*
*	Date: 2/15/2009
*
*   Purpose: The Control handles the keyboard/mouse input for the 
*   user-to-model interaction
*/

#include "../include/Control.h"

//singleton pattern based on http://www.inquiry.com/techtips/cpp_pro/10min/10min0200.asp
	Control* Control::pinstance = 0;// initialize pointer	

	Control* Control::getInstanceOf()//returns instance of Control2
	{
		if (pinstance == 0)  // is it the first call?
		{  
		pinstance = new Control; // create sole instance
		}
		return pinstance; // address of sole instance
	}//end get instance of

	//#############################################################
	void Control::keyboardHandle(unsigned char key, int x, int y)
    {//use unit vector model.direction to increment velocity in correct direction
	//by using a differential velocity passed to updateVelocity
	//v[2] is forward look vector, v[1] is upward vector, v0 is strafe vector
	
	Control *theControl = Control::getInstanceOf();
	
   switch(key)
   {    
   case '`': //regiters stage handler's keyboard function
   case '~': theControl->setIsActive(!theControl->getIsActive());
	*(theControl->getHandleNumberPtr())=2;//tells director to check for active stagehand handler
	
             break;

   case 'w': 
   case 'W': theControl->changeForwardVelocity(1.0f);
   //diffVel.v[2] +=1;
   //cout << diffVel.v[2] <<" z forward pressed"<< endl;
   //cout << key <<" pressed"<< endl;

             break;
   case 'a':
   case 'A': theControl->changeRightVelocity(-1.f);
   //cout << diffVel.v[0] <<" x  right pressed"<< endl;
   //cout << key <<" pressed"<< endl;

             break;
   case 's':
   case 'S': theControl->changeForwardVelocity(-1.0f);
   //cout << diffVel.v[2] <<" z forward pressed"<< endl;
   //cout << key <<" pressed"<< endl;

             break;
   case 'd':
   case 'D': theControl->changeRightVelocity(1.0f);
   //cout << diffVel.v[0] <<" x  right pressed"<< endl;
   //cout << key <<" pressed"<< endl;

             break;      
			 
   case 'g':
   case 'G': theControl->stopVelocity();
   break;
   //cout << diffVel.v[0] <<" x  right pressed"<< endl;
   //cout << key <<" pressed"<< endl;
   case 'o':
   case 'O': 
   theControl->changePitch(theControl->InvertedLook*0.2);
   
             break; 
   case 'l':
   case 'L': 
   
   theControl->changePitch(-theControl->InvertedLook*0.2);
   
             break; 
   case 'k':
   case 'K': 
   
   theControl->changeHeading(theControl->InvertedHorizLook*0.2);
   
             break;
   case ';':
   case ':': 
   
   theControl->changeHeading(-theControl->InvertedHorizLook*0.2);
   
             break;	 
   case 'q':
   case 'Q':  exit(0);
			 break;   
   default:
            cout << key << endl;
   } // End switch
    //Note on diffVel, it is treated as 
	//a straight 3d coord, with +-z always  
	//forwards and backwards of current direction
	//y up/down, x left/right
	
    }//end keyboardhandle
    
    void Control::specialHandle(int key, int x, int y)
    {
         
    }

	//#############################################################
	void Control::mouseHandle(int x, int y)
    {
		 Control *theControl = Control::getInstanceOf();
		 
		 //locals
		 int CenterX = theControl->CenterX;
		 int CenterY = theControl->CenterY;
		 int DeadZone = theControl->DeadZone;
		 int lastX = theControl->lastX;
		 int lastY = theControl->lastY;
		 
		 
		 if(x > (CenterX + DeadZone) || x < (CenterX - DeadZone) || y > (CenterY + DeadZone) || y < (CenterY - DeadZone))
		{
		 
		 float diffMouse = 0;
		 
		 if(x < lastX)
		 {
			diffMouse = (lastX - x);
			theControl->changeHeading(theControl->InvertedHorizLook*diffMouse*0.08);
		 }
		 
		 else if(x > lastX)
		 {
			diffMouse = (x - lastX);
			theControl->changeHeading(-theControl->InvertedHorizLook*diffMouse*0.08);
		 }
		 
		 if(y < lastY)
		 {
			diffMouse = (lastY - y);
			theControl->changePitch(theControl->InvertedLook*diffMouse*0.08);
		 }
		 
		 else if(y > lastY)
		 {
			diffMouse = (y - lastY);
			theControl->changePitch(-theControl->InvertedLook*diffMouse*0.08);
		 }
		 
		 //cout<<"dM: "<<diffMouse<<endl;
		 //cout<<"pitch: "<<theControl->modelPtr->getDirection().v[0]*9.11<<endl;
		 //cout<<"heading: "<<theControl->modelPtr->getDirection().v[1]*9.11<<endl;
		 theControl->lastX = CenterX;
		 theControl->lastY = CenterY;
		 
		 //theControl->updateDirection();
		 
		glutWarpPointer(CenterX,CenterY);
		}//end if
		 
    }//end mouseHandle

	
	//#############################################################
	void Control::updateVelocity()
	{
	vector3f currentVel = modelPtr->getVelocity();
	vector3f rightVect;
	vector3f upVect(0,1,0);
	//get forward vector from direction.
	calcPoints(modelPtr->getDirection());//sets forward vel
	
	//tempvect.v[1] = 0;//not moving up unless jumping,ignore for now
	vector3f tempvect = normalizeVector(diffVel);
	
	//next use cross(forward) = strafe vector for changing perpendicular vel
	rightVect = crossProduct(upVect,tempvect);
	rightVect = normalizeVector(rightVect);
	
	currentVel.v[0] += (tempvect.v[0]*m_ForwardVelocity); 
	currentVel.v[1] += (tempvect.v[1]*m_ForwardVelocity); 
	currentVel.v[2] += (tempvect.v[2]*m_ForwardVelocity);
	
	currentVel.v[0] += (rightVect.v[0]*m_RightVelocity); 
	currentVel.v[1] += (rightVect.v[1]*m_RightVelocity); 
	currentVel.v[2] += (rightVect.v[2]*m_RightVelocity);
	
	modelPtr->setVelocity(currentVel);
	//vector3f resetVect(0,0,0);
	//setDiffVel(resetVect);
	
	stopVelocity();
	//cout<<"vel updated"<<endl;*/
	}//end updateVelocity

	
	//#############################################################
	void Control::updateDirection()
	{
		//deprecated, replaced by change heading/pitch
	}//end updateDirection

//############################################3	
	void Control::changePitch(float degrees)
{//mostly from nehe tutorial
	
	vector3f oldDir = modelPtr->getDirection();
	
	if(fabs(degrees) < fabs(m_MaxPitchRate))
	{
		// Our pitch is less than the max pitch rate that we 
		// defined so lets increment it.
		oldDir.v[0] += degrees;
	}
	else
	{
		// Our pitch is greater than the max pitch rate that
		// we defined so we can only increment our pitch by the 
		// maximum allowed value.
		if(degrees < 0)
		{
			// We are pitching down so decrement
			oldDir.v[0] -= m_MaxPitchRate;
		}
		else
		{
			// We are pitching up so increment
			oldDir.v[0] += m_MaxPitchRate;
		}
	}

	// We don't want our pitch to run away from us. Although it
	// really doesn't matter I prefer to have my pitch degrees
	// within the range of -360.0f to 360.0f
	
	float degreeLimit = 90.0/9.11f;//compensate for factor of 9.11 in internal degree units
	
	if(oldDir.v[0] > degreeLimit)
	{
		oldDir.v[0] = degreeLimit;
	}
	else if(oldDir.v[0] < -degreeLimit)
	{
		oldDir.v[0] = -degreeLimit;
	}
     //fprintf(stderr,"pitch: %f", m_PitchDegrees);
     
	 modelPtr->setDirection(oldDir);
	 
}//end changePitch 
	
	
//################################################################33	
	void Control::changeHeading(float degrees)
{

	vector3f oldDir = modelPtr->getDirection();
	
	float degreeLimit1 = 90/9.11;
	float degreeLimit2 = 270/9.11;
	
	if(fabs(degrees) < fabs(m_MaxHeadingRate))
	{
		// Our Heading is less than the max heading rate that we 
		// defined so lets increment it but first we must check
		// to see if we are inverted so that our heading will not
		// become inverted.
		
		
		
		if(oldDir.v[0] > degreeLimit1 && oldDir.v[0] < degreeLimit2 || (oldDir.v[0] < -degreeLimit1 && oldDir.v[0] > -degreeLimit2))
		{
			oldDir.v[1] -= degrees;
		}
		else
		{
			oldDir.v[1] += degrees;
		}
	}
	else
	{
		// Our heading is greater than the max heading rate that
		// we defined so we can only increment our heading by the 
		// maximum allowed value.
		if(degrees < 0)
		{
			// Check to see if we are upside down.
			if(oldDir.v[0] > degreeLimit1 && oldDir.v[0] < degreeLimit2 || (oldDir.v[0] < -degreeLimit1 && oldDir.v[0] > -degreeLimit2))
			{
				// Ok we would normally decrement here but since we are upside
				// down then we need to increment our heading
				oldDir.v[1] += m_MaxHeadingRate;
			}
			else
			{
				// We are not upside down so decrement as usual
				oldDir.v[1] -= m_MaxHeadingRate;
			}
		}
		else
		{
			// Check to see if we are upside down.
			if(oldDir.v[0] > degreeLimit1 && oldDir.v[0] < degreeLimit2 || (oldDir.v[0] < -degreeLimit1 && oldDir.v[0] > -degreeLimit2))
			{
				// Ok we would normally increment here but since we are upside
				// down then we need to decrement our heading.
				oldDir.v[1] -= m_MaxHeadingRate;
			}
			else
			{
				// We are not upside down so increment as usual.
				oldDir.v[1] += m_MaxHeadingRate;
			}
		}
	}
	
	// We don't want our heading to run away from us either. Although it
	// really doesn't matter I prefer to have my heading degrees
	// within the range of -360.0f to 360.0f
	
	//float degreeLimit3 = 360/9.11; = 39.5
	
	if(oldDir.v[1] > 39.5)
	{
		oldDir.v[1] -= 39.5;
	}
	else if(oldDir.v[1] < -39.5)
	{
		oldDir.v[1] += 39.5;
	}
	
	//to compensate for 9.11 factor on degrees, mulitply number by 9.11 so it can be compared to standard degrees
	//will divide when used for rotations
	
	//convert to normal degrees for interfacing
	
	modelPtr->setDirection(oldDir);
	//fprintf(stderr,"heading: %f", m_PitchDegrees);
}//end change heading
	
//####################################################################	
	vector3f Control::calcPoints(vector3f angles)
	{
	//convert back to internal degrees: 1/9.11 deg to 1 internal degree 
	
	vector3f forwardVect;
	
	float Matrix[16];
	Quaternion tempQuaternion;
	
	// Make the Quaternions that will represent our rotations
	xRotation.CreateFromAxisAngle(1.0f, 0.0f, 0.0f, angles.v[0]);//pitch
	yRotation.CreateFromAxisAngle(0.0f, 1.0f, 0.0f, angles.v[1]);//heading
	
	// Combine the pitch and heading rotations and store the results in q
	tempQuaternion = xRotation * yRotation;
	tempQuaternion.CreateMatrix(Matrix);

	// Let OpenGL set our new prespective on the world!
	glMultMatrixf(Matrix);
	
	// Create a matrix from the pitch Quaternion and get the j vector 
	// for our direction.
	xRotation.CreateMatrix(Matrix);
	forwardVect.v[1] = Matrix[9];

	// Combine the heading and pitch rotations and make a matrix to get
	// the i and j vectors for our direction.
	tempQuaternion = yRotation * xRotation;
	tempQuaternion.CreateMatrix(Matrix);
	forwardVect.v[0] = Matrix[8];
	forwardVect.v[2] = Matrix[10];

	//grab forward vector values
	diffVel.v[0]=forwardVect.v[0];
	diffVel.v[1]=forwardVect.v[1];
	diffVel.v[2]=forwardVect.v[2];


	//phys manager codes
	// Increment our position by the vector
	//m_Position.x += forwardVect.v[0];
	//m_Position.y += forwardVect.v[1];
	//m_Position.z += forwardVect.v[2];

	// Translate to our new position.
	//glTranslatef(-modelPosition.v[0], -modelPosition.v[1], modelPosition.v[2]);
	return forwardVect;
	}//end calcpoints
	
	
	//#########################################################3333
	void Control::changeForwardVelocity(float vel)
{
	if(fabs(vel) < fabs(m_MaxForwardVelocity))
	{
		// Our velocity is less than the max velocity increment that we 
		// defined so lets increment it.
		m_ForwardVelocity += vel;
	}
	else
	{
		// Our velocity is greater than the max velocity increment that
		// we defined so we can only increment our velocity by the 
		// maximum allowed value.
		if(vel < 0)
		{
			// We are slowing down so decrement
			m_ForwardVelocity -= -m_MaxForwardVelocity;
		}
		else
		{
			// We are speeding up so increment
			m_ForwardVelocity += m_MaxForwardVelocity;
		}
	}
}//end change forward
	//#####################################################
	void Control::changeRightVelocity(float vel)
{
	if(fabs(vel) < fabs(m_MaxForwardVelocity))
	{
		// Our velocity is less than the max velocity increment that we 
		// defined so lets increment it.
		m_RightVelocity += vel;
	}
	else
	{
		// Our velocity is greater than the max velocity increment that
		// we defined so we can only increment our velocity by the 
		// maximum allowed value.
		if(vel < 0)
		{
			// We are slowing down so decrement
			m_RightVelocity -= -m_MaxForwardVelocity;
		}
		else
		{
			// We are speeding up so increment
			m_RightVelocity += m_MaxForwardVelocity;
		}
	}
}//end change right
		//#####################################################
	void Control::stopVelocity()
	{
		m_RightVelocity = 0.0;
		m_ForwardVelocity = 0.0;
	}//end stop
	
	
	//####################################################################
	vector3f Control::normalizeVector(vector3f vect)
	{
	float magnitude = sqrt(vect.v[0]*vect.v[0]+vect.v[1]*vect.v[1]+vect.v[2]*vect.v[2]);
	vector3f unitVect;
	unitVect.v[0]=vect.v[0]/magnitude;
	unitVect.v[1]=vect.v[1]/magnitude;
	unitVect.v[2]=vect.v[2]/magnitude;
	return unitVect;
	}//end normailzevector
	
	//####################################################################
	vector3f Control::crossProduct(vector3f vect1, vector3f vect2)
	{
	vector3f tempvect;
	tempvect.v[0] = vect1.v[1]*vect2.v[2]-vect1.v[2]*vect2.v[1];
	tempvect.v[1] = vect1.v[2]*vect2.v[0]-vect1.v[0]*vect2.v[2];
	tempvect.v[2] = vect1.v[0]*vect2.v[1]-vect1.v[1]*vect2.v[0];
	return tempvect;
	}//end normailzevector

	
